#include <string.h>
#include <assert.h>
#include "nvs_flash.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"

#include "app_storage.h"

static bool inited = false;
static QueueHandle_t appstorage_evt_queue = NULL;

struct storage_info {
    enum {
        APP_STORAGE_SPEED,
        APP_STORAGE_ACCEL,
        APP_STORAGE_CURPOS,
        APP_STORAGE_MAXPOS
    } type;
    uint64_t val;
};

static int _app_storage_set_motor_speed(uint32_t speed);
static int _app_storage_set_motor_accel(uint32_t accel);
static int _app_storage_set_motor_curpos(uint64_t curpos);
static int _app_storage_set_motor_maxpos(uint64_t maxpos);

int app_storage_set_motor_speed(uint32_t speed)
{
    struct storage_info info;
    info.type = APP_STORAGE_SPEED;
    info.val = speed;

    if (xQueueSend(appstorage_evt_queue, &info, pdMS_TO_TICKS(500)) != pdPASS) {
        ESP_LOGE("STORAGE", "Failed to send event to app storage task.");
        return -1;
    }
    return 0;
}
int app_storage_set_motor_accel(uint32_t accel)
{
    struct storage_info info;
    info.type = APP_STORAGE_ACCEL;
    info.val = accel;

    if (xQueueSend(appstorage_evt_queue, &info, pdMS_TO_TICKS(500)) != pdPASS) {
        ESP_LOGE("STORAGE", "Failed to send event to app storage task.");
        return -1;
    }
    return 0;
}

int app_storage_set_motor_curpos(uint64_t curpos)
{
    struct storage_info info;
    info.type = APP_STORAGE_CURPOS;
    info.val = curpos;

    if (xQueueSendFromISR(appstorage_evt_queue, &info, NULL) != pdPASS) {
        return -1;
    }
    return 0;
}
int app_storage_set_motor_maxpos(uint64_t maxpos)
{
    struct storage_info info;
    info.type = APP_STORAGE_MAXPOS;
    info.val = maxpos;

    if (xQueueSend(appstorage_evt_queue, &info, pdMS_TO_TICKS(500)) != pdPASS) {
        ESP_LOGE("STORAGE", "Failed to send event to app storage task.");
        return -1;
    }
    return 0;
}

static void appstorage_task(void* arg)
{
    struct storage_info info;

    for (;;) {
        if (xQueueReceive(appstorage_evt_queue, &info, portMAX_DELAY)) {
            switch (info.type) {
                case APP_STORAGE_SPEED:
                    _app_storage_set_motor_speed((uint32_t)info.val);
                    ESP_LOGI("STORAGE", "Setting speed to %lld.", info.val);
                    break;
                case APP_STORAGE_ACCEL:
                    _app_storage_set_motor_accel((uint32_t)info.val);
                    ESP_LOGI("STORAGE", "Setting accel to %lld.", info.val);
                    break;
                case APP_STORAGE_CURPOS:
                    _app_storage_set_motor_curpos(info.val);
                    ESP_LOGI("STORAGE", "Setting curpos to %lld.", info.val);
                    break;
                case APP_STORAGE_MAXPOS:
                    _app_storage_set_motor_maxpos(info.val);
                    ESP_LOGI("STORAGE", "Setting maxpos to %lld.", info.val);
                    break;
                default:
                    ESP_LOGE("STORAGE", "Unknown event type %lld.", info.val);
                    break;
            }
        }
    }
}

void app_storage_init(void)
{
    esp_err_t ret = nvs_flash_init_partition("storage");
    if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
        ESP_ERROR_CHECK(nvs_flash_erase_partition("storage"));
        ret = nvs_flash_init_partition("storage");
        if (ret != ESP_OK) {
            ESP_LOGE("STORAGE", "Error (%s) initializing NVS partition.", esp_err_to_name(ret));
            return;
        }
    }

    ESP_LOGI("STORAGE", "Initialized NVS partition.");

    appstorage_evt_queue = xQueueCreate(10, sizeof(struct storage_info));
    xTaskCreate(appstorage_task, "appstorage_task", 4096, NULL, 10, NULL);
    inited = true;
}

int app_storage_get_motor_info(struct motor_info *info)
{
    if (!inited)
        return -1;

    int ret = 0;

    nvs_handle my_handle;
    esp_err_t err = nvs_open_from_partition("storage", "motor", NVS_READWRITE, &my_handle);
    if (err != ESP_OK) {
        ESP_LOGE("STORAGE", "Error (%s) opening NVS handle!", esp_err_to_name(err));
        return -1;
    }

    err = nvs_get_u32(my_handle, "speed", &info->speed);
    if (err != ESP_OK) {
        ESP_LOGE("STORAGE", "Error (%s) reading speed!", esp_err_to_name(err));
        ret |= err;
    }

    err = nvs_get_u32(my_handle, "accel", &info->accel);
    if (err != ESP_OK) {
        ESP_LOGE("STORAGE", "Error (%s) reading accel!", esp_err_to_name(err));
        ret |= err;
    }

    err = nvs_get_u64(my_handle, "curpos", &info->curpos);
    if (err != ESP_OK) {
        ESP_LOGE("STORAGE", "Error (%s) reading curpos!", esp_err_to_name(err));
        ret |= err;
    }

    err = nvs_get_u64(my_handle, "maxpos", &info->maxpos);
    if (err != ESP_OK) {
        ESP_LOGE("STORAGE", "Error (%s) reading maxpos!", esp_err_to_name(err));
        ret |= err;
    }

    nvs_close(my_handle);

    return ret;
}

static int _app_storage_set_motor_speed(uint32_t speed)
{
    if (!inited)
        return -1;

    nvs_handle my_handle;
    esp_err_t err = nvs_open_from_partition("storage", "motor", NVS_READWRITE, &my_handle);
    if (err != ESP_OK) {
        ESP_LOGE("STORAGE", "Error (%s) opening NVS handle!", esp_err_to_name(err));
        return -1;
    }

    err = nvs_set_u32(
        my_handle,
        "speed",
        speed
    );
    if (err != ESP_OK) {
        ESP_LOGE("STORAGE", "Error (%s) writing speed!", esp_err_to_name(err));
    }

    nvs_close(my_handle);

    return 0;
}

static int _app_storage_set_motor_accel(uint32_t accel)
{
    if (!inited)
        return -1;

    nvs_handle my_handle;
    esp_err_t err = nvs_open_from_partition("storage", "motor", NVS_READWRITE, &my_handle);
    if (err != ESP_OK) {
        ESP_LOGE("STORAGE", "Error (%s) opening NVS handle!", esp_err_to_name(err));
        return -1;
    }

    err = nvs_set_u32(my_handle, "accel", accel);
    if (err != ESP_OK) {
        ESP_LOGE("STORAGE", "Error (%s) writing accel!", esp_err_to_name(err));
    }

    nvs_close(my_handle);

    return 0;
}

static int _app_storage_set_motor_curpos(uint64_t curpos)
{
    if (!inited)
        return -1;

    nvs_handle my_handle;
    esp_err_t err = nvs_open_from_partition("storage", "motor", NVS_READWRITE, &my_handle);
    if (err != ESP_OK) {
        return -1;
    }

    err = nvs_set_u64(
        my_handle,
        "curpos",
        curpos
    );

    nvs_close(my_handle);

    return 0;
}

static int _app_storage_set_motor_maxpos(uint64_t maxpos)
{
    if (!inited)
        return -1;

    nvs_handle my_handle;
    esp_err_t err = nvs_open_from_partition("storage", "motor", NVS_READWRITE, &my_handle);
    if (err != ESP_OK) {
        ESP_LOGE("STORAGE", "Error (%s) opening NVS handle!", esp_err_to_name(err));
        return -1;
    }

    err = nvs_set_u64(my_handle, "maxpos", maxpos);
    if (err != ESP_OK) {
        ESP_LOGE("STORAGE", "Error (%s) writing maxpos!", esp_err_to_name(err));
    }

    nvs_close(my_handle);

    return 0;
}